#include "servo.h"

// Function implementations for servo control

float servo1_angle = 140.0f; // 0~270
float servo3_angle = 120.0f; // 50~220

Servo_Node XY_Servo = {
    .x_angle = 140.0f,
    .y_angle = 120.0f,
    .x_delta = 0,
    .y_delta = 0
};






void set_servo3_cmp(uint32_t cmp)  // PWMB CH0
{
    DL_TimerG_setCaptureCompareValue(PWM_B_INST, cmp, GPIO_PWM_B_C0_IDX);
}

void set_servo3_angle(float angle)    // 0~270 占空比取值2.5% ~ 12.5% 重装载25000
{
    // 限制角度范围
    if(angle < 50) angle = 50;
    if(angle > 220) angle = 220;
    
    // 将角度映射到占空比：0°->2.5%, 270°->12.5%
    float duty_cycle = 2.5f + (angle / 270.0f) * 10.0f;
    
    // 计算duty_value：占空比 * 重装载值 / 100
    uint32_t duty_value = (uint32_t)(duty_cycle * 25000 / 100.0f);
    
    // 按照init的方式设置比较值：25000 - duty_value
    DL_TimerG_setCaptureCompareValue(PWM_B_INST, 25000 - duty_value, GPIO_PWM_B_C0_IDX);
}

void set_servo1_cmp(uint32_t cmp)  // PWMA CH0
{
    DL_TimerG_setCaptureCompareValue(PWM_A_INST, cmp, GPIO_PWM_A_C0_IDX);
}

void set_servo1_angle(float angle)    // 0~270 占空比取值2.5% ~ 12.5% 重装载25000
{
    // 限制角度范围
    if(angle < 0) angle = 0;
    if(angle > 270) angle = 270;
    
    // 将角度映射到占空比：0°->2.5%, 270°->12.5%
    float duty_cycle = 2.5f + (angle / 270.0f) * 10.0f;
    
    // 计算duty_value：占空比 * 重装载值 / 100
    uint32_t duty_value = (uint32_t)(duty_cycle * 25000 / 100.0f);
    
    // 按照init的方式设置比较值：25000 - duty_value
    DL_TimerG_setCaptureCompareValue(PWM_A_INST, 25000 - duty_value, GPIO_PWM_A_C0_IDX);
}

void servo_init(void)
{
    set_servo1_angle(servo1_angle);
    set_servo3_angle(servo3_angle);
}
void servo_proc(void)
{
    if (menu_mode == 2)
    {
        set_servo1_angle(servo1_angle);
        set_servo3_angle(servo3_angle);
    }
    
}

void servo_test_proc(void)
{
    static float angle1 = 45.0f;
    static float angle3 = 45.0f;

    // 每次调用增加5度
    angle1 += 5.0f;
    angle3 += 5.0f;

    // 如果超过270度，重置为0度
    if (angle1 > 225.0f) angle1 = 0.0f;
    if (angle3 > 225.0f) angle3 = 0.0f;

    // 设置角度
    set_servo1_angle(angle1);
    set_servo3_angle(angle3);
}